#include "WPILib.h"
#include "Robot2489.h"

// green HSL filters
	
static Range hueRange        = {60, 130}  ;  ////////////HSL////////////
static Range saturationRange = {60, 255} ;  ///color range for image///
static Range luminanceRange  = {125, 255};  /////////detection/////////

static char  *fileName       = "image.jpg";
/*
//red HSL filters
static Range hueRange        = {0, 20}  ;  ////////////HSL////////////
static Range saturationRange = {60, 255} ;  ///color range for image///
static Range luminanceRange  = {125, 255};  /////////detection/////////
*/
double minArea = 300.0;

void Robot2489::StoreParticles(){

	double x = 0;
	double y = 0;
	double height = 0;
	double width = 0;

	double area = 0;

	//Debug output
	fprintf(debugFile, "List of particles: \n\n");
	
	for(int i = 0; i < reportCount; i++){
		imaqMeasureParticle(convexedImage->GetImaqImage(),
							i,
							false,
							IMAQ_MT_CENTER_OF_MASS_X,
							&x);
		imaqMeasureParticle(convexedImage->GetImaqImage(),
							i,
							false,
							IMAQ_MT_CENTER_OF_MASS_Y,
							&y);
		imaqMeasureParticle(convexedImage->GetImaqImage(),
							i,
							false,
							IMAQ_MT_AREA,
							&area);
		imaqMeasureParticle(convexedImage->GetImaqImage(),
							i,
							false,
							IMAQ_MT_BOUNDING_RECT_HEIGHT,
							&height);
		imaqMeasureParticle(convexedImage->GetImaqImage(),
							i,
							false,
							IMAQ_MT_BOUNDING_RECT_WIDTH,
							&width);
		
		
		//push particles into list, after an area sanity check									
		if (area > minArea){
			particlePoint currentPoint = {Robot2489::kTop, x, y, height, width};
			particles->push_back(currentPoint);

			//Debug Output
			fprintf(debugFile, "Point %d: (%f, %f)\n", i, x, y);
		}
	}
	fprintf(debugFile, "List of points completed \n \n");
}

void Robot2489::OrderParticles(){

	int left = 0;
	int right = 0;
	int top = 0;
	int bottom = 0;
	double xMin = 0;
	double yMin = 0;
	double xMax = 0;
	double yMax = 0;

	if( (reportCount <= 4) && (reportCount >= 1)){
		for(int i = 0; i < reportCount; i++){
			if (particles->at(i).xCenter < xMin) {
				left = i;
				xMin = particles->at(i).xCenter;
			}
			if (particles->at(i).xCenter > xMax) {
				right = i;
				xMax = particles->at(i).xCenter;
			}
			if (particles->at(i).yCenter > yMax) {
				bottom = i;
				yMax = particles->at(i).yCenter;
			}
			if (particles->at(i).yCenter < yMin) {
				top = i;
				yMin = particles->at(i).yCenter;
			}
		}

		//sort particles
		particles->at(top).position = Robot2489::kTop;
		particles->at(left).position = Robot2489::kLeft;
		particles->at(right).position = Robot2489::kRight;
		particles->at(bottom).position = Robot2489::kBottom;
		
		screen->PrintfLine(DriverStationLCD::kUser_Line5, "Four particles :)");
		screen->UpdateLCD();
	}
	
	else {
		//More Debug Output
		screen->PrintfLine(DriverStationLCD::kUser_Line5, "Not four particles :(");
		screen->UpdateLCD();
	}

}
void Robot2489::DetectRectangles(){
	
    
	rawImage->Write("raw.bmp");
	//apply some filters
	
	imaqColorThreshold(thresholdedImage->GetImaqImage(), //output
			           rawImage->GetImaqImage(),         //input
			           50,       //Value to replace pixels within threshold
			           IMAQ_HSL, //Color plane to use
			           &hueRange, 
			           &saturationRange, 
			           &luminanceRange); //a color threshold (for green light)
	
	thresholdedImage->Write("thresholded.bmp");
	fprintf(debugFile, "Error in imaqColorThreshold: %s", imaqGetErrorText(imaqGetLastError()));
	
	//changeType(thresholdedImage, IMAQ_IMAGE_U8);
	imaqLabel2(labeledImage->GetImaqImage(),     //output
			   thresholdedImage->GetImaqImage(), //input
			   true,  //use connectivity8
			   NULL); //label the particles before running convex hull
	
	imaqConvexHull(convexedImage->GetImaqImage(), //output
				   labeledImage->GetImaqImage(),  //input
			       true); //Apply convex hull operation (fill in empty spaces)
	
		       
	convexedImage->Write("convexed.bmp");
	//Count and record all particles in convexedImage
	imaqCountParticles(convexedImage->GetImaqImage(), //input
					   true,                          //use connectivity8
					   &reportCount);                 //record number of particles
	
	StoreParticles(); //store particles in a vector, filtering for size
	OrderParticles(); //
	//convexedImage->Write("convexed.png");
	
}

void Robot2489::InitializeImages(){

	rawImage = camera->GetImage();
	thresholdedImage = new ColorImage(IMAQ_IMAGE_U8);
	labeledImage = new ColorImage(IMAQ_IMAGE_U8);
	convexedImage = new ColorImage(IMAQ_IMAGE_U8);
	sizeFilteredImage = new ColorImage(IMAQ_IMAGE_U8);
}



void Robot2489::DeleteImages(){
	//memory cleanup
	delete rawImage;
	delete thresholdedImage;
	delete convexedImage;
	delete labeledImage;
}


void Robot2489::changeType(ColorImage *image, ImageType type){
	imaqCast(NULL,                    //change source image
			 image->GetImaqImage(),   //source image
			 type,                    //type to change into
			 NULL,                    //default settings
			 -1);                     //no shift
}

Robot2489::particlePoint Robot2489::AccessParticle(particlePos position){
	for(int i = 0; i < reportCount; i++){
		if (particles->at(i).position == position) return particles->at(i);
	}
}

void Robot2489::ProcessLocation(void){

	double pixelHeight = 0;
	double pixelWidth = 0;
	double pixelTurretAngle = 0;
	double midHeight = 0;
	double height = 0;
	double backBoardHeight = 0;
	

	switch (reportCount) {

	case 4: {
		pixelHeight = AccessParticle(Robot2489::kBottom).yCenter -
			AccessParticle(Robot2489::kTop).yCenter;
		pixelWidth  = AccessParticle(Robot2489::kRight).xCenter - 
			AccessParticle(Robot2489::kLeft).xCenter;
		pixelTurretAngle = 320.0 - AccessParticle(Robot2489::kTop).xCenter;
		height = BOT_TO_TOP_HEIGHT;
		backBoardHeight = BOTTOM_CENTER_HEIGHT;
	}

	case 3: {
		midHeight = (AccessParticle(Robot2489::kLeft).yCenter +
				 AccessParticle(Robot2489::kRight).yCenter)/2;
		pixelHeight = AccessParticle(Robot2489::kBottom).yCenter - midHeight;
		pixelWidth  = AccessParticle(Robot2489::kRight).xCenter - 
			AccessParticle(Robot2489::kLeft).xCenter;
		height = BOT_TO_MID_HEIGHT;
		backBoardHeight = BOTTOM_CENTER_HEIGHT;
	}
	case 2: {
		//will finish
	}
	case 1: {
		pixelHeight = AccessParticle(Robot2489::kBottom).Height;
		pixelWidth = AccessParticle(Robot2489::kBottom).Width;
		height = BACKBOARD_HEIGHT;
		backBoardHeight = BOTTOM_EDGE_HEIGHT;
	} }
	
	double heightAngle = TO_RAD*(FOV/640.0)*pixelHeight;
	double widthAngle  = TO_RAD*(FOV/640.0)*pixelWidth;
	
	if (heightAngle != 0){	
		double a  = tan(PI - heightAngle);
		double b  = height;
		double c  = a * (CAM_HEIGHT - backBoardHeight) * (height - CAM_HEIGHT + backBoardHeight);

		//my calculations, derived using triangles made of backboard and camera, and assumed angles obtained via
		//pixel width are accurate
		distance    = (-b - sqrt(pow(b, 2) + 4 * a * c) ) / (2 * a);

		//Debug output
		fprintf(debugFile, "Calculated distance: %f \n", distance);

		/*	robotAngle  = acos(sqrt(pow(1.0 / cos(widthAngle), 2) / 2.0 -
			pow(tan(widthAngle), 2) * (pow(WIDTH/distance, 2) + 
			pow(WIDTH / 4.0 / distance, 2))));*/
		turretAngle = TO_RAD*(FOV/640.0)*pixelTurretAngle; 
	
	}
	//a direction of view makes one backboard appear higher than the other, and this can be used
	if (AccessParticle(Robot2489::kLeft).yCenter < AccessParticle(Robot2489::kRight).yCenter){
		side = Robot2489::kLeftSide;
		//screen->PrintfLine(DriverStationLCD::kUser_Line5,"Left");
		screen->UpdateLCD();
	}
	else{
		side = Robot2489::kRightSide;
		//screen->PrintfLine(DriverStationLCD::kUser_Line5,"Right");
		screen->UpdateLCD();
	}
}


void Robot2489::TestSingleImage(string imageIn)
{
	InitializeImages();
	rawImage = new HSLImage::HSLImage(imageIn.c_str());
	
	//Debug Output
	fprintf(debugFile, "Testing with file %s \n", imageIn.c_str());

	DetectRectangles();
	ProcessLocation();
	DeleteImages();
}

void Robot2489::TestImages()
{
	string image = "/20Ft2.bmp";
	TestSingleImage(image);

//	image = "/20Ft2.jpg";
//	TestSingleImage(image.c_str());
}

void Robot2489::RecordCurrentImage(){
	
	//initialize counter, open counter file, and initialize int-string converter
	int imageNumber = 0;
	char *imageCounterFile = "imageCounter.txt";
	fstream imageCounter(imageCounterFile);
	ostringstream convert;

	//Increment image number in file
	imageCounter >> imageNumber;
	imageNumber++;
	imageCounter.seekg(ios_base::beg);
	imageCounter << imageNumber;

	//Convert imageNumber to an image file name, and record image into the file
	convert << "image" << imageNumber << ".png";
	const char *imageFile = convert.str().c_str();
	rawImage->Write(imageFile);
	
}
